#ifndef ABSTRACTHANDTRACKERTEST_HPP
#define ABSTRACTHANDTRACKERTEST_HPP

#include <cv.h>
#include <highgui.h>

#include "abstractProbabilityTransformer.hpp"
#include "abstractFilter.hpp"
#include "resourceTracker.h"
#include "faceDetector.h"
#include "handLocaliser.hpp"
#include "HIString-inl.hpp"
#include "HIGeom-inl.hpp"

#include "AbstractHandTracker.hpp"

#include <iostream>
//TODO clean up includes

#include "gtest.h"

//FIX clean up VISUALISATION
//NOTE: hiDependenciesForTestFramework also sets HI_TEST_VISUALISATION
//which if set will make tests compile to show images of the things they test.

using namespace cv;
using namespace hi;

//FIX: This is only really a pseudo-header!

namespace {
	
	// The fixture for testing class PerPixelProbabilityTransformerTest.
	class AbstractHandTrackerTest : public ::testing::Test {
	public:
		
		AbstractHandTracker *ht;
		ResourceTracker *rt;
		
		cv::Ptr< cv::VideoCapture > capPtr;
		cv::Ptr< VideoPointTagSequence > seqPtr;
		cv::Mat frame;
		
		HandLocaliser* hl;
		
		cv::Rect trackingRect;
		
		double errorThreshold;
		int errorStatus;
		
		//TODO RAT
		//int localisedHandInFrame;
		
	protected:
		
		virtual void SetUp();
		
		virtual void TearDown();
		
		virtual void setHandTracker(int trackerCode)=0;
		
		int initTrackerTest(const char* videoNameString);
		
		void testTracker(int trackerCode, const char* videoNameString);
	};
	
}; //namespace


void ::AbstractHandTrackerTest::SetUp() {
	
	errorStatus = 0;
	
	rt = new ResourceTracker( "../config/runtimeSettings/cfg.xml" );
	rt->loadCategory("TestData");
}

void ::AbstractHandTrackerTest::TearDown() {
	delete rt;
	delete ht;
}

int ::AbstractHandTrackerTest::initTrackerTest(const char* videoNameString) {
	
	capPtr = rt->getFileVideoCapture( videoNameString );
	seqPtr = rt->getPointTags( videoNameString );
	
	int tagCount = seqPtr->tagCount;
	
	if (tagCount < 1) {
		CV_Error( CV_StsError, "No tags!"); //throws exception!
	}
	
	hl = new HandLocaliser();
	
	int localisationframeNumber = 0; //NOTE: frame zero is never tracked as it is always used by 
	
	for ( ; localisationframeNumber < tagCount; localisationframeNumber++ ) {
		try {
			(*capPtr) >> frame;
			seqPtr->getNextPointTag();
			trackingRect = hl->localiseHand( frame );
			
			cout << "Face found" << endl;
			
			std::cerr << localisationframeNumber + 1 << ";" ;
			
			break;
		}
		catch (cv::Exception e) {
			if ( e.code == HI_ERR_NOFACEFOUND ) {
				continue;
			} else {
				throw;
			}
		}
	}
	
	errorThreshold  = std::sqrt( trackingRect.width*trackingRect.width + trackingRect.height*trackingRect.height ) / 2.0; //Radial distance
	
	forceInside( frame.size(), trackingRect );

	return localisationframeNumber;
}

void ::AbstractHandTrackerTest::testTracker(int trackerCode, const char* videoNameString ) {
	
	// -- prepare test variables and parameters -- //
	
	RecordProperty( "videoNameString ", videoNameString);
	int localisationFrameNumber = initTrackerTest( videoNameString ); //returns number of first frame where face was found
	int firstTrackingFrame = localisationFrameNumber+1;
	
	setHandTracker( trackerCode );
	
	int tagCount = seqPtr->tagCount;
	int tagsTracked = tagCount - localisationFrameNumber;
	
	double errorSum = 0;
	double maxErr = 0;
	
	double squareErrorSum = 0; 
	
	// -- Prepare timing data -- //
	
	double totalTrackTime = 0;
	double totalSquareTrackTime = 0;
	
	for( int frameCount = firstTrackingFrame; frameCount<tagCount; frameCount++) {
		// -- get next frame -- //
		(*capPtr) >> frame;
		
		//VISUALISATION
		{
			Mat imgToDraw;
			frame.copyTo(imgToDraw);
			HiVisualiser::windowMatrixMap["handtracker"] = imgToDraw;
		}
		
		// -- tracking frame -- //
		
		double trackTime = (double)cvGetTickCount();
		trackingRect = ht->track( frame );
		trackTime = (double)cvGetTickCount() - trackTime;
		
		totalTrackTime += trackTime;
		totalSquareTrackTime += trackTime*trackTime;
		
		//compute tracking error from ground truth
		Point groundTruth = seqPtr->getNextPointTag();
		
		Point trackingRectCentre = Point( trackingRect.x + trackingRect.width/2, trackingRect.y + trackingRect.height/2 );
		
		int dX = groundTruth.x - trackingRectCentre.x;
		int dY = groundTruth.y - trackingRectCentre.y;
		
		double frameSquareError =  dX*dX + dY*dY ;
		double frameError = std::sqrt( frameSquareError );
		
		if ( maxErr < frameError ) {
			maxErr = frameError;
		}
		
		if ( frameError > errorThreshold ) {
			errorStatus = (errorStatus < 1) ? 1 : errorStatus;
			if ( frameError > 2*errorThreshold) {
				errorStatus = (errorStatus < 2) ? 2 : errorStatus;
				if ( frameError > 3*errorThreshold) {
					errorStatus = 3; //Max
				}
			}
		}
		
		
		errorSum += frameError;
		squareErrorSum += frameSquareError;
		
		//VISUALISATION
		{
			Mat imgToDraw = HiVisualiser::windowMatrixMap["handtracker"];
			
			//tracked location
			circle(imgToDraw, Point( trackingRect.x + trackingRect.width/2, trackingRect.y + trackingRect.height/2 ), 1, Scalar(0, 255, 0), 2);
			
			//tracking rect
			rectangle( imgToDraw, Point( trackingRect.x, trackingRect.y ), Point( trackingRect.x + trackingRect.width, trackingRect.y + trackingRect.height ),
					  Scalar( 0, 255, 0 ) );
			
			//tagged ground truth
			circle(imgToDraw, groundTruth, 1, Scalar(0, 0, 255), 1);
			//errorStatus 1-to-2 bound
			circle(imgToDraw, groundTruth, (int) (errorThreshold), Scalar(0, 0, 255), 1);
			//errorStatus 1-to-2 bound
			circle(imgToDraw, groundTruth, (int) (errorThreshold*2), Scalar(0, 0, 255), 1);
			//errorStatus 2-to-3 bound
			circle(imgToDraw, groundTruth, (int) (errorThreshold*3), Scalar(0, 0, 255), 1);
			HiVisualiser::refreshWindow("handtracker");
		}
		
		waitKey(1);
//		char c = waitKey(0);
//		if( c == 27 ) break;
	}
	
	double timeVariance = (totalSquareTrackTime - totalTrackTime*totalTrackTime/(double)tagsTracked)/(double)tagsTracked;
	
	double avgError = errorSum/tagsTracked;
	
	//Note: properties should have no spaces in names to give well-formed xml
	RecordProperty( "TagsTracked", tagsTracked );
	RecordProperty( "BaseErrorThreshold", errorThreshold );
	RecordProperty( "ErrorStatus", errorStatus );
	RecordProperty( "AvgPositionError ", avgError);
	double timeTrackingOnly = totalTrackTime/((double)cvGetTickFrequency()*1e6);
	RecordProperty( "TimeTrackingOnly", timeTrackingOnly );
	RecordProperty( "TrackingFPS", tagsTracked/timeTrackingOnly );
	RecordProperty( "MaxErr", maxErr );
	
	//To make it easy to record data to a csv -- //NOTE: Also data recorded during init
	std::cerr << tagsTracked << ";";
	std::cerr << errorThreshold << ";";
	std::cerr << errorStatus << ";";
	std::cerr << avgError << ";";
	std::cerr << timeTrackingOnly << ";";
	std::cerr << tagsTracked/timeTrackingOnly << ";";
	std::cerr << maxErr << ";";
	std::cerr << std::sqrt(squareErrorSum/tagsTracked) << ";";
	std::cerr << std::sqrt(timeVariance)/((double)cvGetTickFrequency()*1e6) << std::endl;
	
	ASSERT_LE(errorStatus, 1); //Consider 1 acceptable
}


// -- Main Method -- //

int main(int argc, char **argv) {
	::testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}


// -- Example test -- //

//
//TEST_F(AbstractHandTrackerTest, LKGREYaaBPMSaaOF0 ) {
//	testTracker( 0, "locVid1c" );
//}

#endif